/**
 * @file Pose2D.h
 * @author rj.wang (rj.wang@aotbot.com)
 * @brief 平面位姿
 * @version 0.1
 * @date 2024-11-04
 * @copyright Copyright (c) 2024
 */

#ifndef __COMMON_PLANNING_POSE2D_H_
#define __COMMON_PLANNING_POSE2D_H_

#include <cmath>
#include <vector>
#include <deque>
#include <set>

#include "angle.h"

namespace common {
namespace planning {

inline double Normalize(double phi) {
  using namespace ::common::base;
  return atan2(sin(Angle16::from_rad(phi)), cos(Angle16::from_rad(phi)));
}

template <typename T>
class Pose2D {
 public:
  Pose2D()
      : x_(0),
        y_(0),
        phi_(0.0) {}

  explicit Pose2D(const T x)
      : x_(x),
        y_(0),
        phi_(0.0)  {}

  Pose2D(const T x, const T y)
      : x_(x),
        y_(y),
        phi_(0.0)  {}

  Pose2D(const T x, const T y, const double phi) : x_(x), y_(y)  {
    phi_ = Normalize(phi);
  }

  Pose2D(const Pose2D<T> &pose)
      : x_(pose.X()),
        y_(pose.Y()),
        phi_(pose.Phi()) {}

  ~Pose2D() = default;

  Pose2D<T> operator+(const Pose2D<T> &pose) const {
    using namespace ::common::base;
    auto cosphi = cos(Angle16::from_rad(phi_));
    auto sinphi = sin(Angle16::from_rad(phi_));
    double x = x_ + pose.X() * cosphi - pose.Y() * sinphi;
    double y = y_ + pose.X() * sinphi + pose.Y() * cosphi;
    double phi = Normalize(phi_ + pose.Phi());

    return Pose2D(x, y, phi);
  }

  bool operator==(const Pose2D<T> &pose) const {
    return (fabs(this->X() - pose.X()) < 1e-6) &&
           (fabs(this->Y() - pose.Y()) < 1e-6);
  }

  bool operator!=(const Pose2D<T> &pose) const {
    return !(*this == pose);
  }

  bool operator<(const Pose2D<T> &pose) const {
    return (this->X() < pose.X()) ||
           (this->X() == pose.X() && this->Y() < pose.Y());
  }

  Pose2D<T> &operator=(const Pose2D<T> &pose) {
    x_ = pose.X();
    y_ = pose.Y();
    phi_ = Normalize(pose.Phi());
    return *this;
  }

  Pose2D<T> operator*(double v) const {
    double x = x_ * v;
    double y = y_ * v;

    return Pose2D(x, y, phi_);
  }

  Pose2D<T> operator/(double v) const {
    double x = x_ / v;
    double y = y_ / v;

    return Pose2D(x, y, phi_);
  }

  double EuclidDistance(const Pose2D<T> &pose) const {
    return sqrt((pose.X() - x_) * (pose.X() - x_) +
                (pose.Y() - y_) * (pose.Y() - y_));
  }

  inline T X() const { return x_; }

  inline T Y() const { return y_; }

  inline double Phi() const { return phi_; }

  inline void SetX(const T x) { x_ = x; }

  inline void SetY(const T y) { y_ = y; }

  void SetPhi(const double phi) {
    phi_ = Normalize(phi);
  }

  void SetPose2D(const T x, const T y, const double phi) {
    SetX(x);
    SetY(y);
    SetPhi(phi);
  }

  bool IsEqual(const Pose2D<T> &pose) const {
    if (fabs(pose.X() - x_) < 1e-6 && fabs(pose.Y() - y_) < 1e-6 &&
        fabs(pose.Phi() - phi_) < 1e-6) {
      return true;
    } else {
      return false;
    }
  }

  bool IsNear(const Pose2D<T> &pose, const double threshold) const {
    if (this->EuclidDistance(pose) < threshold) {
      return true;
    } else {
      return false;
    }
  }

 private:
  T x_;
  T y_;
  double phi_;
};

}  // namespace planning
}  // namespace common

typedef common::planning::Pose2D<double> Pose;
typedef std::vector<Pose> PoseVec;
typedef std::deque<Pose> PoseDq;
typedef std::set<Pose> PoseSet;

typedef common::planning::Pose2D<int32_t> Cell;
typedef std::vector<Cell> CellVec;
typedef std::deque<Cell> CellDq;
typedef std::set<Cell> CellSet;

namespace common {
namespace planning {
/**
 * @brief Bresenham画线算法
 */
inline CellVec GetLinePoints(const Cell& start, const Cell& end) {
  // bresenham --> get pixels on the line((x0, y0) --> (x1, y1))
  CellVec points{};
  int64_t x0 = start.X();
  int64_t y0 = start.Y();
  int64_t x1 = end.X();
  int64_t y1 = end.Y();
  bool steep = abs(y1 - y0) > abs(x1 - x0);
  if (steep) {
    std::swap(x0, y0);
    std::swap(x1, y1);
  }
  if (x0 > x1) {
    std::swap(x0, x1);
    std::swap(y0, y1);
  }
  int64_t deltax = x1 - x0;
  int64_t deltay = abs(y1 - y0);
  int64_t error = deltax / 2;
  int64_t y = y0;
  int64_t ystep = y0 < y1 ? 1 : -1;
  for (int64_t x = x0; x <= x1; ++x) {
    // steep is false --> pixel is (x, y); steep is true --> pixel is (y, x)
    points.emplace_back(steep ? y : x, steep ? x : y);
    error = error - deltay;
    if (error < 0) {
      y = y + ystep;
      error = error + deltax;
    }
  }
  return points;
}

}  // namespace planning
}  // namespace common

#endif  // __COMMON_PLANNING_POSE2D_H_
